#include "platform_controller_server/patrol.h"


Patrol::Patrol(const ros::NodeHandle& nh,const ros::NodeHandle& nh_private)
	: nh_(nh)
	, nh_private_(nh_private)
	, isFirstGlobalPath_(false)
	, isEnablePatrol_(false)
	, update_goal_(false)
	, sendTargetFlag_(false)
	, time_(500)
	, rest_time_(500)
	, scale_(2)
	, sendTargetTime_(ros::Time::now())
	, currentTime_(ros::Time::now())
	, arrivedTime_(ros::Time::now())
	, waitSeconds_(ros::Duration(3.0))
	, patrol_mode_(0)
	, location_status_(false)
	, navigation_points_save_path_("")
	, navigation_points_load_path_("")
	, csv_save_path_("")
	, update_map_flag_(false)
{
	
	nh_private_.param("navigation_points_save_path",navigation_points_save_path_,navigation_points_save_path_);
	nh_private_.param("navigation_points_load_path",navigation_points_load_path_,navigation_points_load_path_);
	nh_private_.param("csv_file_load_path",csv_save_path_,csv_save_path_);
	
	ROS_INFO("navigation points save path: %s",navigation_points_save_path_.c_str());

	//订阅
	sub_loadNavigationPoints = nh_.subscribe<std_msgs::String>("loadPoints",1,&Patrol::onLoadPatrolPoints,this);  //接收系统保存的导航点
	sub_save_patrol_point    = nh_.subscribe<std_msgs::String>("save_patrol_point",1,&Patrol::onSavePatrolPoints,this);
	sub_add_patrol_point     = nh_.subscribe<std_msgs::String>("add_patrol_point",1,&Patrol::onAddPatrolPoint,this);

	sub_nav_map              = nh_.subscribe<nav_msgs::OccupancyGrid>("/map",1,&Patrol::onNavGridMapCallback,this);
	sub_laser_scan           = nh_.subscribe<sensor_msgs::LaserScan>("/scan",1,&Patrol::onLaserScanCallback,this);

	sub_single_circle        = nh_.subscribe<std_msgs::Empty>("single_patrol",1,&Patrol::onSingleCirclePatrolCallback,this);
	sub_movebase_feedback    = nh_.subscribe<move_base_msgs::MoveBaseActionFeedback>("/move_base/feedback",1,&Patrol::onGoalFeedbackCallback,this);
	sub_movebase_result      = nh_.subscribe<move_base_msgs::MoveBaseActionResult>("/move_base/result",1,&Patrol::onGoalResultCallback,this);

	sub_enablePatrol         = nh_.subscribe<std_msgs::Bool>("isEnablePatrol",1,&Patrol::enablePartol_callback,this);
	sub_getPatrolCommand     = nh_.subscribe<platform_controller_msgs::PatrolTask>("/patrol_task",1,&Patrol::onPatrolCommand_callback,this);
	sub_isArrivedTarget      = nh_.subscribe<platform_controller_msgs::IsArrivedTarget>("isArrivedTarget", 1,&Patrol::isArrivedTarget_callback,this);
	sub_firstGlobalPath      = nh_.subscribe<nav_msgs::Path>("/move_base/NavfnROS/plan", 1,&Patrol::firstGlobalPath_callback,this);
	sub_task_completed       = nh_.subscribe<std_msgs::Bool>("task_completed",1,&Patrol::onTaskCompletedCallback,this);


	sub_load_patients        = nh_.subscribe<std_msgs::String>("load_patients",1,&Patrol::onLoadCSVFile,this);
	pub_patients             = nh_.advertise<platform_controller_msgs::Patients>("patients",1);


	//发布一个导航目标点
	pub_currentTarget        = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",1);
	pub_tts                  = nh_.advertise<std_msgs::String>("tts",1);
	pub_cancelCurrentTarget  = nh_.advertise<std_msgs::Bool>("cancelCurrentNavigationTask",1,true);

	// 发布日志信息
	pub_log_                 = nh_.advertise<std_msgs::String>("log",1);

	pub_patrol_task_completed_ = nh_.advertise<std_msgs::Empty>("/patrol_task_completed",1);

	//发布操作结果
	pub_operation_result_      = nh_.advertise<std_msgs::String>("/operation_result",1);

	//发布一个融合地图，位置与激光数据的图
	pub_compose_img          = nh_.advertise<sensor_msgs::Image>("compose_img",1,true);

	//timer
	sendTargetTimer_ = nh_.createTimer(ros::Duration(1.0),&Patrol::timerEvent,this);

	// boost::thread visiual_thread(boost::bind(&Patrol::visuial_map,this));

    tf::TransformListener listener;
	ros::Rate loop_rate(10);
	while(ros::ok())
	{
		tf::StampedTransform transform;
		try 
		{
			listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
			
			robot_pose_.header.stamp = ros::Time::now();
			robot_pose_.header.frame_id = "/map";

			robot_pose_.pose.position.x = transform.getOrigin().getX();
			robot_pose_.pose.position.y = transform.getOrigin().getY();
			robot_pose_.pose.position.z = transform.getOrigin().getZ();

			robot_pose_.pose.orientation.x = transform.getRotation().getX();
			robot_pose_.pose.orientation.y = transform.getRotation().getY();
			robot_pose_.pose.orientation.z = transform.getRotation().getZ();
			robot_pose_.pose.orientation.w = transform.getRotation().getW();

			location_status_ = true;
		}
		catch (tf::TransformException ex)
		{
			location_status_ = false;
			//ROS_ERROR("-------> %s", ex.what());
		}
		ros::spinOnce();
		loop_rate.sleep();
	}
}

Patrol::~Patrol()
{

}

//设置到达目标点后的等待时间
void Patrol::setWaitSeconds(int seconds)
{
	waitSeconds_ = ros::Duration(seconds);
}

//获取当前要前往的目标点
platform_controller_msgs::NavigationPoint Patrol::getCurrentTarget()
{
	return navigationPoint_;
}

void Patrol::enablePartol_callback(const std_msgs::Bool::ConstPtr &msg)
{
	isEnablePatrol_ = msg->data;

	//关闭导航
	if(!isEnablePatrol_)
	{
		sendTargetFlag_ = false;
		update_goal_ = false;                            //测试

		std_msgs::Bool cancelCurrentTarget;
		cancelCurrentTarget.data = false;
		pub_cancelCurrentTarget.publish(cancelCurrentTarget);
	}
}


void Patrol::sendTarget()
{
	if(0 == patrol_mode_)   //shun xu mo shi 
	{
		//先取出队列的最前面的点，然后将该点添加到对尾
		navigationPoint_ = patrolNavigationPoints_.front();
		std::cout<<"point voice: "<<navigationPoint_.tts<<std::endl;

		std::vector<platform_controller_msgs::NavigationPoint>::iterator iter = patrolNavigationPoints_.begin();
		std::cout << "going to the: " << iter->point_name.c_str()<<std::endl;
		geometry_msgs::PoseStamped posestamped;
		
		posestamped.pose.position.x = iter->pose.position.x;
		posestamped.pose.position.y = iter->pose.position.y;
		posestamped.pose.position.z = iter->pose.position.z;

		posestamped.pose.orientation.x = iter->pose.orientation.x;
		posestamped.pose.orientation.y = iter->pose.orientation.y;
		posestamped.pose.orientation.z = iter->pose.orientation.z;
		posestamped.pose.orientation.w = iter->pose.orientation.w;

		pub_currentTarget.publish(posestamped);

		sendTargetTime_ = ros::Time::now();

		//将该点添加到队尾，并从队首删除
		patrolNavigationPoints_.push_back(navigationPoint_);
		patrolNavigationPoints_.erase(patrolNavigationPoints_.begin());
		
		ROS_INFO("send sequence point over");
		
	}
	else if(1 == patrol_mode_)                                 //random
	{
		int index = rand()%patrolNavigationPoints_.size();
		assert(index < patrolNavigationPoints_.size());

		navigationPoint_ = patrolNavigationPoints_.at(index);
		std::cout << "going to the: " << navigationPoint_.point_name.c_str()<<std::endl;

		geometry_msgs::PoseStamped posestamped;

		posestamped.pose = navigationPoint_.pose;
		// posestamped.pose.position.x = navigationPoint_.position[0];
		// posestamped.pose.position.y = navigationPoint_.position[1];
		// posestamped.pose.position.z = navigationPoint_.position[2];

		// posestamped.pose.orientation.x = navigationPoint_.orientation[0];
		// posestamped.pose.orientation.y = navigationPoint_.orientation[1];
		// posestamped.pose.orientation.z = navigationPoint_.orientation[2];
		// posestamped.pose.orientation.w = navigationPoint_.orientation[3];
	
		pub_currentTarget.publish(posestamped);
		
		sendTargetTime_ = ros::Time::now();

		ROS_INFO("send random point over");
	}
	else if(2 == patrol_mode_)       //Single Circle
	{
		if(patrolNavigationPoints_.size() > 0)
		{
			navigationPoint_ = patrolNavigationPoints_.front();
			geometry_msgs::PoseStamped posestamped;
			posestamped.header.frame_id = "map";
			posestamped.header.stamp = ros::Time::now();
			posestamped.pose = navigationPoint_.pose;
			pub_currentTarget.publish(posestamped);
			sendTargetTime_ = ros::Time::now();

			ROS_INFO("send normal partol point: %d",navigationPoint_.id);
			patrolNavigationPoints_.erase(patrolNavigationPoints_.begin());
		}
	}
} 


void Patrol::onAddPatrolPoint(const std_msgs::String::ConstPtr& msg)
{
	tts_notification_msg_.data = "添加送餐点";
	pub_tts.publish(tts_notification_msg_);

	if(location_status_)
	{
		navigationPoint_.header.seq = totalNavigationPoints_.size();
		navigationPoint_.header.frame_id = "/map";
		navigationPoint_.header.stamp = ros::Time::now();

		navigationPoint_.id = totalNavigationPoints_.size();
		navigationPoint_.point_name = "";
		navigationPoint_.type = 1;
		navigationPoint_.tts = "";
		navigationPoint_.room_num = atoi(msg->data.c_str());
		navigationPoint_.pose = robot_pose_.pose;
		totalNavigationPoints_.push_back(navigationPoint_);
	}
	else
	{
		navigationPoint_.header.seq = totalNavigationPoints_.size();
		navigationPoint_.header.frame_id = "/odom";
		navigationPoint_.header.stamp = ros::Time::now();

		navigationPoint_.id = totalNavigationPoints_.size();
		navigationPoint_.point_name = "";
		navigationPoint_.type = 0;
		navigationPoint_.tts = "";
		navigationPoint_.room_num = atoi(msg->data.c_str());
		navigationPoint_.pose = geometry_msgs::Pose();
		totalNavigationPoints_.push_back(navigationPoint_);
	}
	ROS_INFO("Add patrol point, id: %d, room_num: %d",navigationPoint_.id,navigationPoint_.room_num);
}

void Patrol::onLoadPatrolPoints(const std_msgs::String::ConstPtr &msg)
{
	operation_response_.operation_type = LOAD_PATROL_POINTS;

	tts_notification_msg_.data = "正在导入送餐点";
	pub_tts.publish(tts_notification_msg_);
	ros::Duration(2).sleep();

	ROS_INFO("patrol clear old points");
	totalNavigationPoints_.clear();

	char navigation_file[256]={0};
	sprintf(navigation_file,"%s/%s",navigation_points_load_path_.c_str(),msg->data.c_str());

	//Open navigation points file
	FILE* fp = fopen(navigation_file, "r"); // 非 Windows 平台使用 "r"
	if(!fp)
	{
		tts_notification_msg_.data = "送餐点文件不存在，请检查";
		pub_tts.publish(tts_notification_msg_);


		operation_response_.result = 0;
		operation_response_.verbose = "送餐点文件不存在，请检查";

		std_msgs::String operation_response_msg;
		operation_response_msg.data = operation_response_.toJsonString();
		pub_operation_result_.publish(operation_response_msg);

		return;
	}
	char readBuffer[65536];
	FileReadStream is(fp, readBuffer, sizeof(readBuffer));
 
	Document dom;
	dom.ParseStream(is);

	if (dom.HasMember("navigation_points") && dom["navigation_points"].IsArray())
	{
		const rapidjson::Value& navigation_points = dom["navigation_points"];

		for (int i = 0; i < navigation_points.Size(); ++i) 
		{
			platform_controller_msgs::NavigationPoint nav_point;
			const rapidjson::Value& point     = navigation_points[i];

			const rapidjson::Value& header    = point["header"];
			nav_point.header.seq              = header["seq"].GetInt();
			nav_point.header.frame_id         = header["frame_id"].GetString();

			const rapidjson::Value& stamp     = header["stamp"];
			nav_point.header.stamp.sec        = stamp["sec"].GetInt();
			nav_point.header.stamp.nsec        = stamp["nsec"].GetInt();


			nav_point.id                      = point["id"].GetInt();
			nav_point.room_num                = point["room_num"].GetInt();
			nav_point.type                    = point["type"].GetInt();
			nav_point.point_name              = point["point_name"].GetString();
			nav_point.tts                     = point["tts"].GetString();

			const rapidjson::Value& pose = point["pose"];
			const rapidjson::Value& position = pose["position"];
			const rapidjson::Value& orientation = pose["orientation"];
			
			nav_point.pose.position.x         = position["x"].GetFloat();
			nav_point.pose.position.y         = position["y"].GetFloat();
			nav_point.pose.position.z         = position["z"].GetFloat();

			nav_point.pose.orientation.x      = orientation["x"].GetFloat();
			nav_point.pose.orientation.y      = orientation["y"].GetFloat();
			nav_point.pose.orientation.z      = orientation["z"].GetFloat();
			nav_point.pose.orientation.w      = orientation["w"].GetFloat();

			totalNavigationPoints_.push_back(nav_point);

			// std::cout<<"patrol point id: "<<nav_point.id<<" room num: "<<nav_point.room_num <<std::endl;
		}
	}
	fclose(fp);

	ROS_INFO("Add %d points to vector",(int)totalNavigationPoints_.size());

	tts_notification_msg_.data = "送餐点导入成功";
	pub_tts.publish(tts_notification_msg_);

	operation_response_.result = 1;
	operation_response_.verbose = "送餐点导入成功";

	std_msgs::String operation_response_msg;
	operation_response_msg.data = operation_response_.toJsonString();
	pub_operation_result_.publish(operation_response_msg);
}


void Patrol::onLoadCSVFile(const std_msgs::String::ConstPtr& msg)
{	
	operation_response_.operation_type = LOAD_PATIENTS;//加载病人信息

	char file_name[256]={0};
	sprintf(file_name,"%s/%s",csv_save_path_.c_str(),msg->data.c_str());
	std::cout<<"csv file: "<<file_name<<std::endl;

	//读取病人的相关信息
	try
	{
		csv_parer_ptr_ = std::shared_ptr<csv::Parser>(new csv::Parser(file_name));
	}
	catch(std::string error)
	{
		operation_response_.result = 0;
		operation_response_.verbose = "病人信息文件不存在，请检查";

		std_msgs::String operation_response_msg;
		operation_response_msg.data = operation_response_.toJsonString();
		pub_operation_result_.publish(operation_response_msg);

		return;
	}

	patientInfoVec_.patients.clear();
	//获取文件头信息
	std::vector<std::string> header = csv_parer_ptr_->getHeader();
	std::cout<<"header size: "<<header.size()<<" row count: "<<csv_parer_ptr_->rowCount()<<std::endl;

	for(int i=0;i<csv_parer_ptr_->rowCount();i++)
	{
		platform_controller_msgs::Patient patient;
		patient.name = (*csv_parer_ptr_)[i][0];
		patient.sex = (*csv_parer_ptr_)[i][1];
		patient.room_num = atoi((*csv_parer_ptr_)[i][2].c_str());

		std::cout<<patient.name<<" "<<patient.room_num<<std::endl;

		for(int j=0;i<totalNavigationPoints_.size();j++)
		{
			
			if(totalNavigationPoints_.at(j).room_num == patient.room_num)
			{
				patient.id = totalNavigationPoints_.at(j).id;
				break;
			}
		}
		std::cout<<patient.name<<" "<<patient.room_num<<" "<<patient.id<<std::endl;

		patientInfoVec_.patients.push_back(patient);
	}

	std::cout<<"Add patient num: "<<patientInfoVec_.patients.size()<<std::endl;

	pub_patients.publish(patientInfoVec_);

	operation_response_.result = 1;
	operation_response_.verbose = "病人信息加载完成";

	std_msgs::String operation_response_msg;
	operation_response_msg.data = operation_response_.toJsonString();
	pub_operation_result_.publish(operation_response_msg);
}




void Patrol::onSavePatrolPoints(const std_msgs::String::ConstPtr& file)
{
	ROS_INFO("Save navigation point to file: %s",file->data.c_str());

	tts_notification_msg_.data = "保存送餐点中,请等待";
	pub_tts.publish(tts_notification_msg_);
	ros::Duration(2).sleep();

	rapidjson::Document doc;
	doc.SetObject();
	rapidjson::Document::AllocatorType& allocator = doc.GetAllocator();

	rapidjson::Value points_array(rapidjson::kArrayType);
	for(int i=0;i<totalNavigationPoints_.size();i++)
	{
		//Construct 
		rapidjson::Value point(rapidjson::kObjectType);

		rapidjson::Value header(rapidjson::kObjectType);
		header.AddMember("seq",totalNavigationPoints_.at(i).header.seq,allocator);

		rapidjson::Value stamp(rapidjson::kObjectType);
		stamp.AddMember("sec",totalNavigationPoints_.at(i).header.stamp.sec,allocator);
		stamp.AddMember("nsec",totalNavigationPoints_.at(i).header.stamp.nsec,allocator);
		header.AddMember("stamp",stamp,allocator);

		rapidjson::Value frame_id(rapidjson::kStringType);
		frame_id.SetString(totalNavigationPoints_.at(i).header.frame_id.c_str(),totalNavigationPoints_.at(i).header.frame_id.length());
		header.AddMember("frame_id",frame_id,allocator);

		point.AddMember("header",header,allocator);

		point.AddMember("id",totalNavigationPoints_.at(i).id,allocator);
		point.AddMember("room_num",totalNavigationPoints_.at(i).room_num,allocator);
		point.AddMember("type",totalNavigationPoints_.at(i).type,allocator);

		rapidjson::Value point_name(rapidjson::kStringType);
		point_name.SetString(totalNavigationPoints_.at(i).point_name.c_str(),totalNavigationPoints_.at(i).point_name.length());
		point.AddMember("point_name",point_name,allocator);

		rapidjson::Value tts(rapidjson::kStringType);
		tts.SetString(totalNavigationPoints_.at(i).point_name.c_str(),totalNavigationPoints_.at(i).point_name.length());
		point.AddMember("tts",tts,allocator);
		// doc.AddMember("tts",totalNavigationPoints_.at(i).tts,allocator);

		rapidjson::Value pose(rapidjson::kObjectType);

		rapidjson::Value position(rapidjson::kObjectType);
		position.AddMember("x",totalNavigationPoints_.at(i).pose.position.x,allocator);
		position.AddMember("y",totalNavigationPoints_.at(i).pose.position.y,allocator);
		position.AddMember("z",totalNavigationPoints_.at(i).pose.position.z,allocator);

		rapidjson::Value orientation(rapidjson::kObjectType);
		orientation.AddMember("x",totalNavigationPoints_.at(i).pose.orientation.x,allocator);
		orientation.AddMember("y",totalNavigationPoints_.at(i).pose.orientation.y,allocator);
		orientation.AddMember("z",totalNavigationPoints_.at(i).pose.orientation.z,allocator);
		orientation.AddMember("w",totalNavigationPoints_.at(i).pose.orientation.w,allocator);

		pose.AddMember("position",position,allocator);
		pose.AddMember("orientation",orientation,allocator);

		point.AddMember("pose",pose,allocator);

		points_array.PushBack(point,allocator);
	}

	doc.AddMember("navigation_points",points_array,allocator);

	rapidjson::StringBuffer s;
	rapidjson::Writer<rapidjson::StringBuffer> writer(s);
	doc.Accept(writer);

	std::ofstream outfile;
	char file_name[128]={0};
	sprintf(file_name,"%s/%s",navigation_points_save_path_.c_str(),file->data.c_str());
	outfile.open(file_name, ios::out | ios::trunc );
	outfile<<s.GetString()<<endl;
	outfile.close();

	tts_notification_msg_.data = "送餐点保存成功";
	pub_tts.publish(tts_notification_msg_);
}


void Patrol::onPatrolCommand_callback(const platform_controller_msgs::PatrolTask::ConstPtr &msg)
{
	ROS_INFO("get patrol command, update patrol point!");

	operation_response_.operation_type = BEGIN_PATROL;

	tts_notification_msg_.data = "接收到送餐指令，即将开始送餐";
	pub_tts.publish(tts_notification_msg_);
	ros::Duration(2).sleep();

	patrolNavigationPoints_.clear();

	for(int i=0;i<msg->index.size();i++)
	{
		for(std::vector<platform_controller_msgs::NavigationPoint>::iterator iter = totalNavigationPoints_.begin(); iter != totalNavigationPoints_.end();iter++)
		{
			//如果房间号对应成功，则将该点
			if(iter->id == msg->index[i])
			{
				patrolNavigationPoints_.push_back(*iter);
				break;
			}
		}
	}

	if(patrolNavigationPoints_.size() == 0)
	{
		ROS_WARN("No target seletced, please check.");

		operation_response_.result = 0;
		operation_response_.verbose = "没有送药点被选中，请先选择送药点";

		std_msgs::String operation_response_msg;
		operation_response_msg.data = operation_response_.toJsonString();
		pub_operation_result_.publish(operation_response_msg);

		return;

	}

	//添加原点
	platform_controller_msgs::NavigationPoint  origin_point;
	origin_point.id = patrolNavigationPoints_.size();
	origin_point.type = 1;
	origin_point.point_name= "";
	origin_point.tts = "";
	origin_point.room_num = 0;
	origin_point.pose.position.x = 0.5;
	origin_point.pose.position.y = 0.4;
	origin_point.pose.position.z = 0;

	origin_point.pose.orientation.x = 0;
	origin_point.pose.orientation.y = 0;
	origin_point.pose.orientation.z = 0;
	origin_point.pose.orientation.w = 1;

	patrolNavigationPoints_.push_back(origin_point);

	ROS_INFO("total %d points have been added to list",(int)patrolNavigationPoints_.size());
	waitSeconds_ = ros::Duration(msg->time);
	patrol_mode_ = msg->mode;

	isEnablePatrol_ = true;



	operation_response_.result = 1;
	operation_response_.verbose = "开始送药中";

	std_msgs::String operation_response_msg;
	operation_response_msg.data = operation_response_.toJsonString();
	pub_operation_result_.publish(operation_response_msg);
}


//添加全部的送餐点，并开始单次导航
void Patrol::onSingleCirclePatrolCallback(const std_msgs::Empty::ConstPtr& msg)
{
	tts_notification_msg_.data = "接收到送餐指令，即将开始送餐";
	pub_tts.publish(tts_notification_msg_);
	ros::Duration(2).sleep();

	patrolNavigationPoints_.clear();

	for(std::vector<platform_controller_msgs::NavigationPoint>::iterator iter = totalNavigationPoints_.begin(); iter != totalNavigationPoints_.end();iter++)
	{
		patrolNavigationPoints_.push_back(*iter);	
	}

	//添加原点
	platform_controller_msgs::NavigationPoint  origin_point;
	origin_point.id = totalNavigationPoints_.size();
	origin_point.type = 1;
	origin_point.point_name= "";
	origin_point.tts = "";
	origin_point.pose.position.x = 0.5;
	origin_point.pose.position.y = 0.4;
	origin_point.pose.position.z = 0;

	origin_point.pose.orientation.x = 0;
	origin_point.pose.orientation.y = 0;
	origin_point.pose.orientation.z = 0;
	origin_point.pose.orientation.w = 1;

	patrolNavigationPoints_.push_back(origin_point);

	//开启送餐循环（单次循环，送餐点运行完成之后回到原点）
	waitSeconds_ = ros::Duration(15);
	patrol_mode_ = 2;
	isEnablePatrol_ = true;

}

//查看运动状态
void Patrol::onGoalFeedbackCallback(const move_base_msgs::MoveBaseActionFeedback::ConstPtr& msg)
{
	if(!isEnablePatrol_)
		return;

	if(msg->status.status != 3)
	{
		//还没有到达目标点
		currentTime_ = ros::Time::now();

		ros::Duration spendTime = currentTime_ - sendTargetTime_;
		
		if(spendTime > ros::Duration(time_))
		{
			//超时	（放弃当前目标点，直接发送下一个目标点）
			ROS_INFO("time out! send new target!");
			isFirstGlobalPath_ = false;
			arrivedTime_ = currentTime_ - waitSeconds_;      
			update_goal_ = true;      
			//sendTarget();	 //发送到新的目标点
		}
		else
		{
			ROS_INFO("rest time: %ds",rest_time_);
			//没有超时（继续等待机器人运动到目标点）
		}
	}
}

//查看运动的结果
void Patrol::onGoalResultCallback(const move_base_msgs::MoveBaseActionResult::ConstPtr& msg)
{
	if(msg->status.status == actionlib_msgs::GoalStatus::SUCCEEDED)
	{
		//到达目标点
		ROS_INFO("patrol arrive target: %d",navigationPoint_.id);
		isFirstGlobalPath_ = false;

		update_goal_ = true;
		arrivedTime_ = ros::Time::now();  

		//arrived at welcome 
		if(0 == navigationPoint_.type)
		{
			// 提示对应房间的人出来取药
			for(int i=0;i<patientInfoVec_.patients.size();i++)
			{
				if(navigationPoint_.room_num == patientInfoVec_.patients.at(i).room_num)
				{
					char tts[512]={0};
					sprintf(tts,"已经到达%d房间，%s,您好，请取药",navigationPoint_.room_num,patientInfoVec_.patients.at(i).name.c_str());
					tts_notification_msg_.data = std::string(tts);
					pub_tts.publish(tts_notification_msg_);
					break;
				}
			}

			// char tts[512]={0};
			// sprintf(tts,"已经到达%d房间，%s,您好，请取药",navigationPoint_.room_num,navigationPoint_.name);
			// tts_notification_msg_.data = std::string(tts);
			// pub_tts.publish(tts_notification_msg_);
			// return;
		}

		//如果待送药的点等于0了，则停止当前的巡逻
		if(patrolNavigationPoints_.size() == 0)
		{
			//停止导航
			isEnablePatrol_ = false;

			//发布当前任务完成的消息
			std_msgs::Empty patrol_task_complete;
			pub_patrol_task_completed_.publish(patrol_task_complete);
		}
	}
}

void Patrol::onNavGridMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& map)
{
	ROS_INFO("New map received");
	boost::mutex::scoped_lock lock(occupancy_map_mutex_);
	occupancy_map_ = map;
	update_map_flag_ = true;
}

void Patrol::onLaserScanCallback(const sensor_msgs::LaserScan::ConstPtr& laserscan)
{
	boost::mutex::scoped_lock lock(laserscan_mutex_);
	laser_scan_ = laserscan;
}


cv::Mat Patrol::convertNavMap2Img(nav_msgs::OccupancyGrid::ConstPtr& occupancy_map)
{
	cv::Mat map_img(occupancy_map->info.width, occupancy_map->info.height,CV_8UC1);
	memcpy(map_img.data,occupancy_map->data.data(),occupancy_map->info.width * occupancy_map->info.height);
	map_img = 255-map_img;
	cv::cvtColor(map_img,map_img,CV_GRAY2BGR);
	return *(&map_img);
}

void Patrol::visuial_map()
{
	ROS_INFO("*************       main        **********");
	tf::TransformListener tf_;
	cv::Mat curr_compose_img;
	while(ros::ok())
	{
		if(update_map_flag_)
		{
			boost::mutex::scoped_lock lock(occupancy_map_mutex_);
			composed_map_img_ = convertNavMap2Img(occupancy_map_);
			update_map_flag_ = false;
		}

		curr_compose_img = composed_map_img_.clone();

		//绘制小车的位置
		{
			//获取激光相对于地图的变换
			bool flag = true;
			tf::StampedTransform transform;
			try {
				tf_.lookupTransform("map", "base_link", ros::Time(0), transform);
			} catch(tf::TransformException& ex){
				ROS_ERROR( "Transform error: %s",ex.what());
				flag = false;
			}

			if(flag)
			{
				Eigen::Vector3d position(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
    			double roll, pitch, yaw;
    			tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);

				// std::cout<<"position: "<<position.transpose()<<std::endl;
				std::cout<<"roll: "<<roll<<" pitch: "<<pitch<<" yaw: "<<yaw<<std::endl;

				int u = (position[0] - occupancy_map_->info.origin.position.x)/occupancy_map_->info.resolution;
				int v = (position[1] - occupancy_map_->info.origin.position.y)/occupancy_map_->info.resolution;

				cv::circle(curr_compose_img,cv::Point(u,v),3,CV_RGB(255,0,0),-1);

				//绘制激光数据
				{
					// try {
					// 	tf_.lookupTransform("map", "base_scan", ros::Time(0), transform);
					// } catch(tf::TransformException& ex){
					// 	ROS_ERROR( "Transform error: %s",ex.what());
					// }

					boost::mutex::scoped_lock lock(laserscan_mutex_);
					if(laser_scan_ != nullptr)
					{
						//ROS_INFO("lidar min: %f, max: %f",laser_scan_->angle_max, laser_scan_->angle_min);
						float angle =laser_scan_->angle_min;
						for(int i=0;i<laser_scan_->ranges.size();i++)
						{
							float u_scan=0.0,v_scan=0.0;
							if(angle >=-M_PI && angle < -M_PI/2)
							{
								//第4象限
								u_scan =   laser_scan_->ranges[i] * sin(M_PI + angle);
								v_scan = - laser_scan_->ranges[i] * cos(M_PI + angle);
							}
							else if(angle >= -M_PI/2 && angle < 0)
							{
								//第一象限
								u_scan = laser_scan_->ranges[i] * cos(M_PI/2 - angle);
								v_scan = laser_scan_->ranges[i] * sin(M_PI/2 - angle);
							}
							else if(angle >=0 && angle < M_PI/2)
							{
								//第二象限
								u_scan = - laser_scan_->ranges[i] * sin(angle);
								v_scan =   laser_scan_->ranges[i] * cos(angle);
							}
							else
							{
								u_scan = - laser_scan_->ranges[i] * sin(M_PI - angle);
								v_scan = - laser_scan_->ranges[i] * cos(M_PI - angle);
							}
							u_scan = u + u_scan/occupancy_map_->info.resolution;
							v_scan = v + v_scan/occupancy_map_->info.resolution;

							//旋转yaw
							// u_scan = u_scan*cos(yaw) - v_scan*sin(yaw);
							// v_scan = u_scan*sin(yaw) - v_scan*cos(yaw);

							angle += laser_scan_->angle_increment;
							cv::circle(curr_compose_img,cv::Point(u_scan,v_scan),2,CV_RGB(0,255,0),-1);
						}
					}
				}


			}
		}

		

		sensor_msgs::ImagePtr feature_img;
		feature_img = cv_bridge::CvImage(std_msgs::Header(), "bgr8", curr_compose_img).toImageMsg();
        pub_compose_img.publish(feature_img);

		boost::this_thread::sleep(boost::posix_time::milliseconds(200));
	}
}


void Patrol::isArrivedTarget_callback(const platform_controller_msgs::IsArrivedTarget::ConstPtr &isArrivedTarget)
{
	if(!isArrivedTarget->isArrived)
	{
		//没有到目标点
		currentTime_ = ros::Time::now();

		//判断是否超时
		ros::Duration spendTime = currentTime_ - sendTargetTime_;
		if(spendTime > ros::Duration(time_))
		{
			//超时	（放弃当前目标点，直接发送下一个目标点）
			ROS_INFO("time out! send new target!");
			isFirstGlobalPath_ = false;
			arrivedTime_ = currentTime_ - waitSeconds_;      
			update_goal_ = true;      
			//sendTarget();	 //发送到新的目标点
		}
		else
		{
			ROS_INFO("rest time: %ds",rest_time_);
			//没有超时（继续等待机器人运动到目标点）
			//pass()
		}
		
	}
	else          
	{
		//到达目标点
		ROS_INFO("patrol arrive target!");
		isFirstGlobalPath_ = false;

		//arrived at welcome 
		if(0 == navigationPoint_.type)
		{
			char tts[1024]={0};
			for(auto iter = patientInfoVec_.patients.begin();iter != patientInfoVec_.patients.end();iter++)
			{
				if(iter->id == navigationPoint_.id)
				{
					sprintf(tts,"已经到达%d房间，%s,请取药",iter->room_num,iter->name.c_str());
					tts_notification_msg_.data = std::string(tts);
					pub_tts.publish(tts_notification_msg_);
					break;
				}
			}
		}

	}
}


void Patrol::firstGlobalPath_callback(const nav_msgs::Path::ConstPtr &global_path)
{
	if(!isFirstGlobalPath_)//是否为第一次收到全局规划的路径　　　　　　　　　　　　　　　　　　　
	{
		time_ = scale_ * global_path->poses.size();
	}
	rest_time_ = scale_ * global_path->poses.size();
	isFirstGlobalPath_ = true;
}



void Patrol::timerEvent(const ros::TimerEvent&e)
{
	if(!isEnablePatrol_)
		return;

	rest_time_--;

	if(patrolNavigationPoints_.size() <= 0)
	{
		ROS_ERROR("no navigation points. Please select navigation points first!");
		return;
	}

	if(!sendTargetFlag_)
	{
		sendTarget();
		sendTargetFlag_ = true;
	}
	
	if(!update_goal_)
		return;
	
 	ros::Time currentTime = ros::Time::now();
	if(currentTime - arrivedTime_ >= waitSeconds_)
	{
		if(patrolNavigationPoints_.size() == 0)
		{
			/*if(!patrol_go_flag_)
			{
				sendTarget();
				patrol_go_flag_ = true;
				update_goal_ = false;
			}*/
		}
		else
		{
			sendTarget();
			update_goal_ = false; 
		}
	}
	else
	{
		ROS_INFO("wait for about %ds",(int)((currentTime - arrivedTime_).toSec()));
	}

}


void Patrol::onTaskCompletedCallback(const std_msgs::Bool::ConstPtr &msg)
{
	if(msg->data)
	{
		update_goal_ = true;
		arrivedTime_ = ros::Time::now();  
	}	
}


//将病人信息转化为json格式字符串
std::string Patrol::patient2Json(const platform_controller_msgs::Patient& patient)
{
	rapidjson::Document doc;
	doc.SetObject();
	rapidjson::Document::AllocatorType& allocator = doc.GetAllocator();

	rapidjson::Value name(rapidjson::kStringType);
	name.SetString(patient.name.c_str(), patient.name.length());
	doc.AddMember("name",name,allocator);

	rapidjson::Value sex(rapidjson::kStringType);
	sex.SetString(patient.sex.c_str(), patient.sex.length());
	doc.AddMember("sex",sex,allocator);

	doc.AddMember("room_num",patient.room_num,allocator);
	doc.AddMember("id",patient.id,allocator);

	rapidjson::StringBuffer s;
	rapidjson::Writer<rapidjson::StringBuffer> writer(s);
	doc.Accept(writer);

	return std::string(doc.GetString());
}

//将巡逻点信息转化为json格式字符串
std::string Patrol::patroPoint2Json(const platform_controller_msgs::NavigationPoint& point)
{
	rapidjson::Document doc;
	doc.SetObject();
	rapidjson::Document::AllocatorType& allocator = doc.GetAllocator();

	rapidjson::Value header(rapidjson::kObjectType);
	header.AddMember("seq",point.header.seq,allocator);

	rapidjson::Value stamp(rapidjson::kObjectType);
	stamp.AddMember("sec",point.header.stamp.sec,allocator);
	stamp.AddMember("nsec",point.header.stamp.nsec,allocator);
	header.AddMember("stamp",stamp,allocator);

	rapidjson::Value frame_id(rapidjson::kStringType);
	frame_id.SetString(point.header.frame_id.c_str(),point.header.frame_id.length());
	header.AddMember("frame_id",frame_id,allocator);

	doc.AddMember("header",header,allocator);

	doc.AddMember("id",point.id,allocator);
	doc.AddMember("room_num",point.room_num,allocator);
	doc.AddMember("type",point.type,allocator);

	rapidjson::Value point_name(rapidjson::kStringType);
	point_name.SetString(point.point_name.c_str(),point.point_name.length());
	doc.AddMember("point_name",point_name,allocator);

	rapidjson::Value tts(rapidjson::kStringType);
	tts.SetString(point.point_name.c_str(),point.point_name.length());
	doc.AddMember("tts",tts,allocator);

	rapidjson::Value pose(rapidjson::kObjectType);

	rapidjson::Value position(rapidjson::kObjectType);
	position.AddMember("x",point.pose.position.x,allocator);
	position.AddMember("y",point.pose.position.y,allocator);
	position.AddMember("z",point.pose.position.z,allocator);

	rapidjson::Value orientation(rapidjson::kObjectType);
	orientation.AddMember("x",point.pose.orientation.x,allocator);
	orientation.AddMember("y",point.pose.orientation.y,allocator);
	orientation.AddMember("z",point.pose.orientation.z,allocator);
	orientation.AddMember("w",point.pose.orientation.w,allocator);

	pose.AddMember("position",position,allocator);
	pose.AddMember("orientation",orientation,allocator);

	doc.AddMember("pose",pose,allocator);

	rapidjson::StringBuffer s;
	rapidjson::Writer<rapidjson::StringBuffer> writer(s);
	doc.Accept(writer);

	return std::string(doc.GetString());
}




int main(int argc,char* argv[])
{
	ros::init(argc,argv,"patrol_node");
	ros::NodeHandle nh,nh_private("~");
	Patrol patrol(nh,nh_private);
	return 0;
}
